/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2010, Jack Pien
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <std_msgs/UInt64.h>
#include <opencv/cvwimage.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/CvBridge.h>
#include <boost/shared_ptr.hpp>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>

#include "airlink_101_camera.hpp"

using namespace __AirlinkNs;

#define ALINK_FIRST_CONTENT_LENGTH_SZ_BYTES 108

/**
 * MJPEG read modes
 */
typedef enum {
    E_REC_MODE_PREHDR,
    E_REC_MODE_WRITEJPEG,
    E_REC_MODE_SCANCONTENT_LENGTH,
    E_REC_MODE_STOP,
} RecMode;

int
AirlinkCapture::Start( int argc, char** argv ) 
{
    ros::init(argc, argv, "airlink_publisher");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise( "/airlink_camera/image", 10 );
    ros::Publisher frame_pub = 
        nh.advertise<std_msgs::UInt64>( "/airlink_camera/frameid", 100 );
    ros::Rate loop_rate(1);

    // Get params
    std::string ip_address("192.168.0.240");
    std::string stream_location("/cgi/mjpg/mjpeg.cgi");
    int port_num=80;
    if( nh.getParam("ip_address", ip_address) ) {
        std::cout << "IP=" << ip_address << std::endl;
    }
    if( nh.getParam("port", port_num) ) {
        std::cout << "port=" << port_num << std::endl;
    }
    if( nh.getParam("stream_location", stream_location) ) {
        std::cout << "stream_location" << stream_location << std::endl;
    } 

    struct sockaddr_in localAddr, addr;
    int sd, rc;

    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(ip_address.c_str());
    addr.sin_port = htons(port_num);

    // Create a socket
    sd = socket(AF_INET, SOCK_STREAM, 0);
    if( sd < 0 ) {
        _ERR( "cannot create socket\n" );
        return 1;
    }

    // Bind any local port number
    localAddr.sin_family = AF_INET;
    localAddr.sin_addr.s_addr = htonl(INADDR_ANY);
    localAddr.sin_port = htons(0);  
    rc = bind(sd, (struct sockaddr *) &localAddr, sizeof(localAddr));
    if(rc<0) {
        _ERR( "cannot bind port TCP\n" );
        return 1;
    }

    // Make a socket connection
    rc = connect(sd, (struct sockaddr *) &addr, sizeof(addr));

    if( rc<0) {
        _ERR("cannot connect to remote port");
        return 1;
    } else {
#define ALINK_READ_BUFFER_SIZE 2048
        int err;
        int contentLength;
        int readLength;
        RecMode recMode=E_REC_MODE_SCANCONTENT_LENGTH;
        char tempbuf[ALINK_READ_BUFFER_SIZE];

        // Send GET request
        sprintf( tempbuf, 
                 "GET %s http/1.1\nAuthorization: Basic YWRtaW46\n\n", 
                 stream_location.c_str() );
        err = send( sd, tempbuf, strlen(tempbuf), 0 );
        if( err != int( strlen(tempbuf) ) ) {
            _ERR("Error sending get request\n");
            return 1;
        } 

        // Start reading first chunk of info
        contentLength = readLength = ALINK_READ_BUFFER_SIZE;
        err = recv( sd, tempbuf, 
                    ALINK_FIRST_CONTENT_LENGTH_SZ_BYTES, MSG_WAITALL );
        
        // Keep reading
        while( err > 0 && nh.ok() ) {
                        
            switch( recMode ) {
#define ALINK_PREJPEG_HDR_SZ_BYTES 28
#define ALINK_BOUNDARY_CONTENT_LENGTH_SZ_BYTES 38
            case E_REC_MODE_PREHDR:
                // We don't care about the first 28 bytes after 0d 0a 0d 0a
                // ie b7 28 00 00 ac 20 ...
                // Don't even know what it stands for
                if( err != ALINK_PREJPEG_HDR_SZ_BYTES ) {
                    _ERR("prehrd: did not receive pre hdr\n");
                    return NULL;
                }

                // Subtract out content length
                contentLength -= err;

                if( contentLength < 0 ) {
                    _ERR("prehdr: err during stream parsing\n");
                    BufferInitPtr();
                    recMode = E_REC_MODE_STOP;
                    contentLength = ALINK_READ_BUFFER_SIZE;
                } else if( contentLength == 0 ) {
                    _ERR("prehdr: why is content length zero\n");
                    BufferInitPtr();
                    recMode = E_REC_MODE_STOP;
                    contentLength = ALINK_READ_BUFFER_SIZE;
                } else {
                    recMode = E_REC_MODE_WRITEJPEG;

                    // Read small chunks into real buffer
                    _ASSERT( _pframe == _pwrite );
                    readLength = (contentLength > ALINK_READ_BUFFER_SIZE) ?
                        ALINK_READ_BUFFER_SIZE : contentLength;
                    err = recv( sd, BufferGetPtr(ALINK_READ_BUFFER_SIZE), 
                                readLength, 0 );
                }
                break;

            case E_REC_MODE_WRITEJPEG:
                
                // Subtract out content length
                contentLength -= err;

                // Update buffer pointer
                BufferIncPtr( err );

                if( contentLength < 0 ) {
                    _ERR( "writejpeg: err during stream parsing" );
                    recMode = E_REC_MODE_STOP;
                    contentLength = ALINK_READ_BUFFER_SIZE;
                    
                } else if( contentLength == 0 ) {

                    // Publish the image
                    unsigned int buf_length = (unsigned int)(_pwrite - _pframe);
                    cv::Mat mat_src( 1, buf_length, CV_8UC(1), _pframe );
                    cv::Mat decoded_img = cv::imdecode( mat_src, -1 );

                    IplImage tmp_img = decoded_img;
                    sensor_msgs::ImagePtr msg = 
                        sensor_msgs::CvBridge::cvToImgMsg( &tmp_img, "bgr8" );
                    pub.publish( msg );
                    std_msgs::UInt64 frame_msg;
                    frame_msg.data = _frameCnt;
                    frame_pub.publish( frame_msg );
                    // loop_rate.sleep();

                    // Start new frame
                    BufferInitPtr();

                    // Get content length of next frame
                    recMode = E_REC_MODE_SCANCONTENT_LENGTH;
                    contentLength = ALINK_BOUNDARY_CONTENT_LENGTH_SZ_BYTES;
                    err = recv( sd, tempbuf, 
                                ALINK_BOUNDARY_CONTENT_LENGTH_SZ_BYTES, 
                                MSG_WAITALL);
                } else {
                    // Continue to read small chunks into real buffer
                    readLength = (contentLength > ALINK_READ_BUFFER_SIZE) ?
                        ALINK_READ_BUFFER_SIZE : contentLength;
                    err = recv( sd, BufferGetPtr(ALINK_READ_BUFFER_SIZE), 
                                readLength, 0 );
                }
                break;

            case E_REC_MODE_SCANCONTENT_LENGTH:
            {
                char* pch;
                char sizeChar[16];

                if( err != ALINK_BOUNDARY_CONTENT_LENGTH_SZ_BYTES &&
                    err != ALINK_FIRST_CONTENT_LENGTH_SZ_BYTES ) {
                    _ERR( "content length: error reading stream\n" );
                    recMode = E_REC_MODE_STOP;
                    break;
                }

                // Look for 'Content-Length: '
                pch = strstr( tempbuf, "Content-Length: " );
                if( pch != NULL ) {
                    int jjj;

                    // Get content length in bytes
                    for( jjj=16; pch[jjj] != 0x0d && pch[jjj] != 0x0a;
                         jjj++ ) {
                        sizeChar[jjj-16] = pch[jjj];
                    }
                    sizeChar[jjj-16] = '\0';

                    // HACK HACK
                    if( jjj-20 > 0 ) {
                        // More than 4 digits for content length
                        err = recv( sd, tempbuf, jjj-20, MSG_WAITALL );
                    }

                    contentLength = atoi(sizeChar);
                    // fprintf( stderr, "Frame %d w/ contentLength %d\n", 
                    //          _frameCnt,
                    //          contentLength );

                    // Increment frame count
                    _frameCnt++;
                    
                    // Read the preheader
                    recMode = E_REC_MODE_PREHDR;

                    // Only read pre jpg hdr size
                    err = recv( sd, tempbuf, ALINK_PREJPEG_HDR_SZ_BYTES, 
                                MSG_WAITALL );
                }
                break;
            }

            case E_REC_MODE_STOP:
                break;

            default:
                _ERR( "Error reading image stream\n" );
                recMode = E_REC_MODE_STOP;
                break;
            }

            // Ros per loop call
            ros::spinOnce();
        }
                
    }
    return 0;
}

int main( int argc, char** argv )
{
    AirlinkCapture alink;
    int err = alink.Start( argc, argv );
    return err;
}
